//My pthreads inclues
#include "pthread.h"
#include "semaphore.h"
#include "unistd.h"
#include <sys/time.h>

//My C++ incdlues
#include <iostream>
#include <cmath>

//My own headers
#include "server.h"
#include "polygon.h"

#define INTERVAL 300000

using namespace std;

extern pthread_mutex_t data_mutex;

string command;
string turn = "t";
string num = "c";
string dist = "d";
string vert_angle = "r";
string show = "s";
string quit = "q";

TCPServer *myServer = 0;

void htondirec(struct directions *d){
    d->dist = htonl(d->dist);
    d->dynamic_on = htonl(d->dynamic_on);
    d->status = htonl(d->status);
    
}

void *NetFace(void*){

    while(1){

        if (myServer->ListenOnClient()) {
            if (myServer->Receive()) {
                
                pthread_mutex_lock(&data_mutex);
                directions tmp = direc;
                pthread_mutex_unlock(&data_mutex);

                htondirec(&tmp);
                myServer->Send(tmp);

            }
        }

        if (myServer->ListenOnHost()) {
            myServer->AcceptConnection();
        }

    }

    return NULL;

}

void printCaptureParams(VideoCapture &cap)
{
    cout << "FOURCC =" << (int) cap.get(CV_CAP_PROP_FOURCC) << endl;
    cout << "FRAME_WIDTH = " << (int) cap.get(CV_CAP_PROP_FRAME_WIDTH) << endl;
    cout << "FRAME_HEIGHT = " << (int) cap.get(CV_CAP_PROP_FRAME_HEIGHT) << endl;
    // cout << "FPS = " << cap.get(CV_CAP_PROP_FPS) << endl;
    cout << "GAIN =" << cap.get(CV_CAP_PROP_GAIN) << endl;
    cout << "BRIGHTNESS =" << cap.get(CV_CAP_PROP_BRIGHTNESS) << endl;
    // cout << "EXPOSURE =" << cap.get(CV_CAP_PROP_EXPOSURE) << endl;
    cout << "CONTRAST =" << cap.get(CV_CAP_PROP_CONTRAST) << endl;
    cout << "SATURATION =" << cap.get(CV_CAP_PROP_SATURATION) << endl;
    
}

void setCaptureParams(VideoCapture &cap)
{
    //cap.set(CV_CAP_PROP_FRAME_WIDTH, 640);
    //cap.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
    // cap.set(CV_CAP_PROP_FPS, 5);
    //cap.set(CV_CAP_PROP_GAIN, 100);
    cap.set(CV_CAP_PROP_BRIGHTNESS, 0.1);
    cap.set(CV_CAP_PROP_CONTRAST, 0.3);
    cap.set(CV_CAP_PROP_SATURATION, 1.0);
}


int main(int argc, char** argv){

    //sem_init(&mutex, 0, 1);

    myServer = new TCPServer();
    myServer->InitiateSocket ( (char *) "1180" );

    direc.dist = 0;
    direc.status = 10;
    direc.dynamic_on = 0;

    pthread_mutex_init(&data_mutex, NULL);
    pthread_t netThread;    
    pthread_create(&netThread, NULL, NetFace, NULL); 

#ifdef __arm__ // pandaboard
#define CAMERA_NUM 3
#else
#define CAMERA_NUM 1
#endif /* __arm__ */
    VideoCapture cap(CAMERA_NUM);
    Mat image;
    RectangleScanner scanner = RectangleScanner();

    printCaptureParams(cap);
    setCaptureParams(cap);

    cout << "New Params:" << endl;

    printCaptureParams(cap);

    cout << "Created objects." << endl;

    struct timeval start;
    struct timeval end;
    int interval = INTERVAL;

    while(1) {

        gettimeofday (&start, NULL);
        
        cap >> image;
        cap >> image;
        cap >> image;
        cap >> image;
        cap >> image;

        processFrame(image, scanner);
        gettimeofday (&end, NULL);
        
        int work_time = (end.tv_sec * 1000000 + end.tv_usec) - (start.tv_sec * 1000000 + start.tv_usec);
#ifdef DEBUG
	cout << "work_time = " << work_time << endl;
#endif
        int sleep_time = interval - work_time;
#ifdef DEBUG
        cout << "sleep_time = " << sleep_time << endl;
#endif
        if (sleep_time > 0) {
            // wait next iteration
            usleep(sleep_time);
        }
    }
     
    return 0;
}
